#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustom9V)
#pragma config(Sensor, S3,     ACCEL,               sensorI2CHiTechnicAccel)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motorH,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*#pragma config(Motor,  mtr_S1_C1_1,     e,             tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     a,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     b,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     c,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     f,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     d,             tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    frontarm,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    backarm,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    dispencer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    Irpullup,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    Irfront,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    Irback,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*/
#define a mtr_S1_C1_1
#define b mtr_S1_C1_2
#define c mtr_S1_C2_1
#define d mtr_S1_C2_2
#ifndef FBRIDGE
#define FBRIDGE
#endif

/*********************************************************************\
*																																	  	*
* PROGRAM: Over Bridge Driver   	      														  *
* VERSION: 1.4																										    *
* PURPOSE: This program is used to get an omni diretional robot       *
*          over the bridge, since the robot doesn't drive straight    *
* AUTHOR: Aaron Jaeger                                                *
* DATE:		 April 2011  	  																					  *g
*																																		  *
* LICENSE: GNU GPL V3 2011                                            *
\**********************************************************************/

#include "CompleteDriver.h"



/*
void back(int speed){
motor[a]=-speed;
motor[b]=-speed;
motor[c]=-speed;
motor[d]=-speed;
}
*/

int getIRsensorValue(byte s) {
  byte i;
  int values[SAMPLES_TOTAL];
  int ignore[SAMPLES_TOTAL];
  for (i=0; i<SAMPLES_TOTAL; i++) {
    ignore[i]=0;
  }

  int sumVals  = 0;
  for (i=0; i<SAMPLES_TOTAL; i++) {
    values[i]=HTPBreadADC(HTPB, s, 10);
    sumVals = sumVals + values[i];
  }
  sumVals = sumVals / SAMPLES_TOTAL;

  byte j;
  for (j=0; j<THROW_AWAY; j++) {
    int worst = -1;
    byte worstIndex = 0;
    for (i=0; i<SAMPLES_TOTAL; i++) {
      if (ignore[i]==0 && abs(sumVals - values[i])>worst) {
        worst = abs(sumVals - values[i]);
        worstIndex = i;
      }
    }
    ignore[worstIndex]=1;
  }

  sumVals  = 0;
  for (i=0; i<SAMPLES_TOTAL; i++) {
    if (ignore[i]==0) {
      sumVals = sumVals + values[i];
    }
  }
  return sumVals/(SAMPLES_TOTAL-THROW_AWAY);
}

#define WIR_VAL_FL getIRsensorValue(WHEEL_IR_FL)
#define WIR_VAL_FR getIRsensorValue(WHEEL_IR_FR)
#define WIR_VAL_BL getIRsensorValue(WHEEL_IR_BL)
#define WIR_VAL_BR getIRsensorValue(WHEEL_IR_BR)

/**
*
* This function lets the NXT know which IR sensor is Falling of the Bridge
* Value of fallingMap changes if the IR senses a drop, or a the surface getting closer
* Each sensor is represented in binary below
* 0001 FL
* 0010 FR
* 0100 BL
* 1000 BR
*
*/
int fallingMap = 0;

int updateFallingMap() {
  fallingMap = 0;
  if (WIR_VAL_FL < EDGE_FL || WIR_VAL_FL > HIGH_EDGE_FL) {      // IR Forward Left is off of bridge
      fallingMap += IR_FL;
  }
  if (WIR_VAL_FR < EDGE_FR || WIR_VAL_FR > HIGH_EDGE_FR) {      // IR Forward Right is off bridge
      fallingMap += IR_FR;
  }
  if (WIR_VAL_BL < EDGE_BL || WIR_VAL_BL > HIGH_EDGE_BL) {      // IR Back Left is off bridge
      fallingMap += IR_BL;
  }
  if (WIR_VAL_BR < EDGE_BR || WIR_VAL_BR > HIGH_EDGE_BR) {      // IR Back Right is off bridge
      fallingMap += IR_BR;
  }
  return fallingMap;
}

/**
* fallingLeft - True when:
*  0001 front left IR is falling
*  0100 Back left IR is Falling
*  0101 Front Left & Back left IR are Falling
*
* fallingRight - True when:
*  0010 Front Right IR is falling
*  1000 Back Right IR is Falling
*  1010 Front Right & Back Right are Falling
*
* allOn - True when:
*   no sides are falling
*  0000 <-- Falling map
*
*/

bool fallingLeft() {
  return (fallingMap ==IR_FL)
  || (fallingMap == IR_BL)
  || (fallingMap == (IR_FL + IR_BL));
}

bool fallingRight () {
  return (fallingMap == (IR_FR + IR_BR)) || (fallingMap == IR_FR) || (fallingMap == IR_BR);
}

bool frontOff () {
  return fallingMap == IR_FR + IR_FL;
}

bool backOff () {
  return fallingMap == IR_BL + IR_BR;
}
bool allOn () {
  return fallingMap == 0;
}

/*
* Below are the two debuging Functions
* showValues - displays all the IR values
* debugFalling - Displays which sides the robot thinks is falling off.
*/
void showValues() {
  nxtDisplayTextLine(2,"FL %d",WIR_VAL_FL);
  nxtDisplayTextLine(3,"FR %d",WIR_VAL_FR);
  nxtDisplayTextLine(4,"BL %d",WIR_VAL_BL);
  nxtDisplayTextLine(5,"BR %d",WIR_VAL_BR);

}

void debugFalling() {
  if(fallingLeft() == true) {
    nxtDisplayTextLine(2,"falling left");
    } else {
    nxtDisplayTextLine(2,"------------");
  }
  if(fallingRight() == true) {
    nxtDisplayTextLine(3,"falling right");
    } else {
    nxtDisplayTextLine(3,"------------");
  }
  if(frontOff() == true) {
    nxtDisplayTextLine(4,"FrontOff");
    } else {
    nxtDisplayTextLine(4,"------------");
  }
  if(backOff() == true) {
    nxtDisplayTextLine(5,"BackOff");
    } else {
    nxtDisplayTextLine(5,"------------");
  }
  if(allOn() == true) {
    nxtDisplayTextLine(7,"allOn");
    } else {
    nxtDisplayTextLine(7,"------------");
  }
}



///////////////////////////////////////////////////////////////////////////////////////////////
//  Following function is the Bridge crosser
//
//
//
///////////////////////////////////////////////////////////////////////////////////////////////
void crossBridge(bool dforward) {
  bool done = false;
  bool dirc = false;  //false -> right, true -> left

int dspeed = (dforward) ? CHARGE_SPD : -CHARGE_SPD;
  while (!done) {
    updateFallingMap();
    //debugFalling();

    forward(dspeed);
    while(allOn() == true) {
      updateFallingMap();
      showValues();
    }
    //PlaySound(soundBeepBeep);
    if (fallingLeft()) {
    dirc = (dforward) ? true : false;
    }
    if (fallingRight()) {
    dirc = (dforward) ? false : true;
    }

    if (dirc) {
      while(nMotorEncoder(c) < R_FIX) {
        frontr(dspeed);
      }
    }
    else {
      while(nMotorEncoder(b) < R_FIX) {
        frontl(dspeed);
      }
    }//if dirc
    wait1Msec(1000);
    stopmot;
    wait1Msec(50);

  }
}

/*
task main() {
nMotorPIDSpeedCtrl[a] = mtrSpeedReg;
nMotorPIDSpeedCtrl[b] = mtrSpeedReg;
nMotorPIDSpeedCtrl[c] = mtrSpeedReg;
nMotorPIDSpeedCtrl[d] = mtrSpeedReg;
eraseDisplay();
bDisplayDiagnostics = false;
eraseDisplay();

while(true) {
showValues();
}
while(true){
crossBridge(true);
}
}
*/
